//
// Created by buenos on 2021/3/6.
//
#include<ros/ros.h>
//#include<ros/console.h>
#include<sensor_msgs/image_encodings.h>
#include<sensor_msgs/Image.h>
#include<cv_bridge/cv_bridge.h>
#include<message_filters/subscriber.h>
#include<message_filters/synchronizer.h>
#include<message_filters/sync_policies/exact_time.h>
#include<message_filters/sync_policies/approximate_time.h>
#include "Tracker.h"

// global variables
ros::Publisher imagePub;
Tracker tracker;

void imgCallback(const sensor_msgs::ImageConstPtr &colorMsg, const sensor_msgs::ImageConstPtr &depthMsg){

//    ros::ROS_DEBUG_ONCE("run into imgCallback"); //colorMsg->encoding
//    std::cout << "run here" << std::endl;
//    std::cout << colorMsg->encoding << std::endl;
//    std::cout << depthMsg->encoding << std::endl;

    // color -> gray
    cv_bridge::CvImageConstPtr imagePtr, depthPtr;
    imagePtr = cv_bridge::toCvCopy(colorMsg, sensor_msgs::image_encodings::MONO8);

    // depthMsg -> depthPtr
    {
        sensor_msgs::Image img;
        img.header = depthMsg->header;
        img.height = depthMsg->height;
        img.width  = depthMsg->width;
        img.is_bigendian = depthMsg->is_bigendian;
        img.step = depthMsg->step;
        img.data = depthMsg->data;
        img.encoding = sensor_msgs::image_encodings::MONO16;
        depthPtr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO16);
    }


    tracker.readImage(imagePtr->image, depthPtr->image, colorMsg->header.stamp.toSec());
    tracker.genOutputImage(imagePtr);

    // pub out img
    imagePub.publish(tracker.outMsg);

}

int main(int argc, char **argv){
    ros::init(argc, argv, "tracking_node");
    ros::NodeHandle n("~");

    // sub
    message_filters::Subscriber<sensor_msgs::Image> imageSub(n, "/camera/color/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depthSub(n, "/camera/depth/image_rect_raw", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> syncPolicy;
    message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10), imageSub, depthSub);
    sync.registerCallback(boost::bind(&imgCallback, _1, _2));

    // pub
    imagePub = n.advertise<sensor_msgs::Image>("feature_image", 100);

    ros::spin();
    return 0;
}